package meshToSolid;

import java.awt.geom.GeneralPath;
import java.io.File;
import java.util.Arrays;
import java.util.Vector;

import grid.Grid3DDepth;
import grid.Grid3DFloat;
import grid.Grid3DReached;
import hdgeom.bezier.Bezier;
import hdgeom.primitives.Box;
import mmesh.MMesh;
import solid.SimpleMCube;
import peasy.PeasyCam;
import processing.core.PApplet;
import processing.core.PGraphics;
import processing.core.PVector;
import simpleGeom.Polygon2D;
import simpleGui.*;

public class VoxelizationHighRes extends PApplet {
	String path = "/Users/dillenburger_b/Documents/Projects/2014 Grotto2/hq/";
	// String pathObject = path + "120acSolid.obj";
	String pathObject = path + "120acSolid.obj";

	String pathValue = pathObject.replace(".obj", ".txt");
	String pathMCubeOut = pathObject.replace(".obj", "vox.obj");
	String pathStream = "/Users/dillenburger_b/Documents/Projects/2014 Grotto2/hq/stream/";
	boolean scanForm = false;
	float dimGrid = 0.005f;// size of voxels in meters

	MMesh mesh;
	Box boundsScan; // Boundingbox of objectfile
	float mirrorX=0.03f;
	// Box boundsInsideInt; // Boundingbox defining voxelized part
	// Box boundsCrop=new Box(-1.97f,-2.00f, 0.02f,0.02f,1.65f,2.9f);
	/*Box boundsCrop = new Box(-1.575f, -1.7534246f, 0.015989847f, 0.01f,
			1.4465753f, 2.14f);*/
	Box boundsCrop = new Box(-1.595f-mirrorX, -1.7534246f, 0.015989847f, 0.01f,
			1.4465753f, 2.17f);

	SimpleMCube mc = new SimpleMCube();
	SimpleMCube mcPreview = new SimpleMCube();

	// PCamera3D cam;
	Grid3DFloat grid;
	int displayMode = 0;
	Gui gui;
	GuiSpinnerGroup sx1, sx2, sy1, sy2, sz1, sz2;
	Grid3DFloat gridPreview;

	boolean flagUpdateInsideOutside;
	boolean flagExport = false;

	// float iso=dimGrid*0.85f;
	float isoFactor = 0.85f;
	float iso = dimGrid * isoFactor;
	PeasyCam cam;

	float rY = 0;
	int step = 4;

	Polygon2D border;

	Grid3DFloat gridHD;
	float dimGridHD = 0.003f;

	public void createGridHD() {
		gridHD = new Grid3DFloat();
		gridHD.nX = (int) (boundsScan.getDimX() / dimGridHD);
		gridHD.nY = (int) (boundsScan.getDimY() / dimGridHD);
		gridHD.nZ = (int) (boundsScan.getDimZ() / dimGridHD);
		gridHD.x1 = boundsScan.x1;
		gridHD.y1 = boundsScan.y1;
		gridHD.z1 = boundsScan.z1;
		gridHD.dim = dimGridHD;
	}

	public void createPlanes() {
		Grid3DFloat plane = new Grid3DFloat(gridHD.nX, 1, gridHD.nZ);
		plane.x1 = gridHD.x1;
		plane.z1 = gridHD.z1;
		plane.dim = gridHD.dim;
		for (int y = 0; y < gridHD.nY; y++) {
			println(y + "/" + gridHD.nY);
			plane.y1 = gridHD.getYFloat(y);
			plane.setValueToAll(1000);
			String fileName = this.getFileName(y);
			plane.saveValues(fileName);
		}
	}

	Grid3DFloat loadPlanes(float y1, float y2) {
		int yint1 = gridHD.getYInt(y1);
		int yint2 = gridHD.getYInt(y2);
		Grid3DFloat gridLoaded = new Grid3DFloat(gridHD.nX, yint2 - yint1,
				gridHD.nZ);
		gridLoaded.x1 = gridHD.x1;
		gridLoaded.z1 = gridHD.z1;
		gridLoaded.dim = gridHD.dim;
		gridLoaded.y1 = y1;
		for (int y = yint1; y < yint2; y++) {
			String fileName = getFileName(y);
			Grid3DFloat plane = new Grid3DFloat(fileName);
			println((y - yint1) + "/" + (yint2 - yint1));
			for (int x = 0; x < gridLoaded.nX; x++) {
				for (int z = 0; z < gridLoaded.nZ; z++) {
					gridLoaded.set(x, y - yint1, z, plane.get(x, 0, z));
				}
			}
		}
		return gridLoaded;
	}

	void savePlanes(Grid3DFloat grid) {
		Grid3DFloat plane = new Grid3DFloat(gridHD.nX, 1, gridHD.nZ);
		plane.x1 = gridHD.x1;
		plane.z1 = gridHD.z1;
		plane.dim = gridHD.dim;
		for (int y = 0; y < grid.nY; y++) {
			plane.y1 = grid.getYFloat(y);
			for (int x = 0; x < grid.nX; x++) {
				for (int z = 0; z < grid.nZ; z++) {
					plane.set(x, 0, z, grid.get(x, y, z));
				}
			}
			String fileName = this.getFileName(y + gridHD.getYInt(grid.y1));
			plane.saveValues(fileName);
		}
	}

	String getFileName(int y) {
		return new String(pathStream + (y + 10000) + ".txt");
	}
	public void setup() {
		// values20140618-114924_dim0.0050.txt
		pathValue = "/Users/dillenburger_b/Documents/Projects/2014 Grotto2/hq/values20140621-093522_dim0.0050.txt";
		size(1000, 600, P3D);
		setupGui();
		cam = new PeasyCam(this, 500);

		constructScanBounds();
		createGridHD();
		 //createPlanes();
		//scanHDContinuous("/Users/dillenburger_b/Documents/Projects/2014 Grotto2/hq/michael04/");
		//
		saveSegmentsHD();
		// defineInsideOutside();
		//loadGrid(pathValue);
		// constructScanBounds();
		// scanContinues("/Users/dillenburger_b/Documents/Projects/2014 Grotto2/hq/michael04/");
		//scanContinues("/Users/dillenburger_b/Documents/Projects/2014 Grotto2/hq/porous all04",grid);

		// scaleX: 0.7994924
		// scaleY: 0.8767123
		// scaleZ: 0.7994924
		//voxelize(scanForm);
	}

	public void setupGui() {
		int py = 20;
		int pyO = 10;
		int x1 = 10;
		int x2 = 30;
		gui = new Gui(this, this.createFont("Arial", 12));
		gui.addGuiLabel(x1, py + pyO, "x1x2: ");
		sx1 = gui.addGuiSpinnerGroup(x2, py, boundsCrop.x1, -3f, 0.2f, 4, 3);
		py += 25;
		sx2 = gui.addGuiSpinnerGroup(x2, py, boundsCrop.x2, -3, 0.2f, 4, 3);
		py += 25;
		gui.addGuiLabel(x1, py + pyO, "y1: ");
		sy1 = gui.addGuiSpinnerGroup(x2, py, boundsCrop.y1, -3, 3, 4, 3);
		py += 25;
		gui.addGuiLabel(x1, py + pyO, "y2: ");
		sy2 = gui.addGuiSpinnerGroup(x2, py, boundsCrop.y2, -3, 3, 4, 3);
		py += 25;
		gui.addGuiLabel(x1, py + pyO, "z1: ");
		sz1 = gui.addGuiSpinnerGroup(x2, py, boundsCrop.z1, -1, 4, 4, 3);
		py += 25;
		gui.addGuiLabel(x1, py + pyO, "z2: ");
		sz2 = gui.addGuiSpinnerGroup(x2, py, boundsCrop.z2, -1, 4, 4, 3);

	}

	public void loadOBJ() {
		mesh = new MMesh();
		mesh.loadOBJ(pathObject);
		mesh.triangulateQuads();
		boundsScan = mesh.getBounds();
		boundsScan.y2 = boundsScan.getDimY() / 3 + boundsScan.y1;
		println("bounds: " + boundsScan);
		boundsScan = boundsScan.getOffsetBox(dimGrid * 2);
	}

	public void addScaled(Grid3DFloat grid) {
		Box source = new Box(grid.x1, -0.151f-0.35f, grid.z1, -1.2f, 0.241f+0.4f, 0.20f);

		// Box sourceInt=constructIntegerBounds(source);
		float zMax = 0.25f;
		int x1 = grid.getXInt(source.x1);
		int y1 = grid.getYInt(source.y1);
		int z1 = grid.getZInt(source.z1);
		int x2 = grid.getXInt(source.x2);
		int y2 = grid.getYInt(source.y2);
		int z3 = grid.getZInt(zMax);

		Grid3DFloat target = new Grid3DFloat(x2 - x1, y2 - y1, z3 - z1);
		//target.setValueToAll(1000);
		target.x1 = grid.getXFloat(x1);
		target.y1 = grid.getYFloat(y1);
		target.z1 = grid.getZFloat(z1);
		target.dim = grid.dim;
		float deltaZ=zMax-source.z2;
		// filling target
		float dY=0.2f/target.dim;
		int yp1=(int)(dY);
		int yp2=target.nY-(int)(dY);
		for (int x = 0; x < target.nX; x++) {
			for (int y = 0; y < target.nY; y++) {
				for (int z = 0; z < target.nZ; z++) {
					float cX = target.getXFloat(x);
					float cY = target.getYFloat(y);
					float cZ = target.getZFloat(z);
					float newZ=zMax;
					if (y<yp1){
						newZ=deltaZ*(sin(PI*y/dY-HALF_PI)+1)/2f+source.z2;
					}
					if (y>yp2){
						newZ=deltaZ*(sin(PI*(target.nY-y)/dY-HALF_PI)+1)/2f+source.z2;
					}
					cZ = map(cZ, target.z1, newZ, source.z1, source.z2);
					if (grid.y1 <= cY && grid.y2() > cY) {
						float v = grid.get(grid.getXInt(cX), grid.getYInt(cY),
								grid.getZInt(cZ));
						
						target.set(x, y, z, v);
					}
				}
			}
		}
		// copying target to grid
		for (int x = 0; x < target.nX; x++) {
			for (int y = 0; y < target.nY; y++) {
				for (int z = 0; z < target.nZ; z++) {
					float targetV = target.get(x, y, z);
					float cY = target.getYFloat(y);
					if (grid.y1 <= cY && grid.y2() > cY) {
						
						float originalV = grid.get(x+x1, y+y1, z+z1);
						if (originalV > targetV) {
							//println("set");
							grid.set(x+x1, y+y1, z+z1,targetV);
						}
					}
				}
			}
		}
	}
	public void addMirrorX(Grid3DFloat grid){
		int mX=grid.getXInt(boundsCrop.x1+mirrorX);
		int newX=grid.getXInt(boundsCrop.x1);
		for (int z=0;z<grid.nZ;z++){
		for (int y=0;y<grid.nY;y++){
		for (int x=newX;x<mX;x++){
			float valueX=grid.get(mX+mX-x, y, z);
			grid.set(x,y,z,valueX);
			}
			}
		}
	}
	public void offset(Grid3DFloat grid){
		float offset=0.02f/grid.dim;
		Grid3DFloat mask=new Grid3DFloat(grid.nX,grid.nY,grid.nZ);
		float iso=grid.dim*0.85f;
		for (int i=0;i<grid.values.length;i++){
			if (grid.values[i]>iso){
				mask.values[i]=0;
			}
			else{
				mask.values[i]=1000000;
			}
		}
		Grid3DDepth.calculateDepth(mask);
		for (int i=0;i<grid.values.length;i++){
			if (mask.values[i]>offset){
				grid.values[i]=iso*2f;
			}
			else{
				
			}
		}
		
	}
	public void voxelize(boolean doScan) {
		if (doScan) {
			loadOBJ();
			scan(dimGrid);

			grid.saveValues(pathValue);
		} else {
			// loadOBJ();
		}

		defineInsideOutside();
		if (!flagExport) {
			mc.marchingCubes(grid.nX, grid.nY, grid.nZ, grid.values,
					grid.dim * 0.85f, grid.dim, grid.x1, grid.y1, grid.z1, null);
		} else {
			mc.marchingCubes(grid.nX, grid.nY, grid.nZ, grid.values,
					grid.dim * 0.85f, grid.dim, grid.x1, grid.y1, grid.z1, path
							+ timeStamp() + ".obj");
			flagExport = false;
		}
	}

	public void scan(float dim) {
		float dimX = boundsScan.getDimX();
		float dimY = boundsScan.getDimY();
		float dimZ = boundsScan.getDimZ();
		int nX = (int) (dimX / dim) + 2;
		int nY = (int) (dimY / dim) + 2;
		int nZ = (int) (dimZ / dim) + 2;
		println("nX: " + nX + "  nY: " + nY + "  nZ: " + nZ);
		grid = new Grid3DFloat(nX, nY, nZ);
		grid.x1 = boundsScan.x1;
		grid.y1 = boundsScan.y1;
		grid.z1 = boundsScan.z1;
		grid.dim = dim;
		// scanning
		MMesh.scanMeshDistance(mesh.xc, mesh.yc, mesh.zc, mesh.triangles,
				boundsScan.x1, boundsScan.y1, boundsScan.z1, nX, nY, nZ, dim,
				dim * 1.6f, grid.values);
	}
	public void updateBoundsCrop(){
		boundsCrop=new Box(sx1.getFloatValue(), sy1
				.getFloatValue(), sz1.getFloatValue(),
				sx2.getFloatValue(), sy2
				.getFloatValue(), sz2.getFloatValue());
	}
	public void constructScanBounds() {
		// bounds: x1:-2.22912 y1:-2.89137 z1:-0.11573 dX:2.35061 dY:1.6964
		// dZ:2.97742
		// new Box(-1.95f,-2.00f, -0.00f,0.02f,1.65f,2.84f);
		boundsScan = new Box();

		/*
		 * boundsVoxel.x1=-2.1f; boundsVoxel.y1=-2.2f; boundsVoxel.z1=-0.11573f;
		 * boundsVoxel.x2=0.05f; boundsVoxel.y2=1.75f; boundsVoxel.z2=3.05f;
		 */

		boundsScan.x1 = -1.6869289f;
		boundsScan.y1 = -1.9375342f;
		boundsScan.z1 = -0.100520186f;
		boundsScan.x2 = 0.047969546f;
		boundsScan.y2 = 1.5430137f;
		boundsScan.z2 = 2.5f;

		// -1.6869289,-1.9375342,-0.100520186
		// 0.047969546,1.5430137,2.4464467
		/*
		 * boundsObject.x1*=1000; boundsObject.x2*=1000; boundsObject.y1*=1000;
		 * boundsObject.y2*=1000; boundsObject.z1*=1000; boundsObject.z2*=1000;
		 */
		boundsScan.offset(dimGrid * 2f);
	}

	public Box constructIntegerBounds(Grid3DFloat grid) {
		return new Box(grid.getXInt(boundsCrop.x1), grid.getYInt(boundsCrop.y1), grid.getZInt(boundsCrop.z1),
				grid.getXInt(boundsCrop.x2) + 1, grid.getYInt(boundsCrop.y2) + 1,
				grid.getZInt(boundsCrop.z2) + 1);
	}

	public void loadGrid(String fileName) {
		grid = new Grid3DFloat(fileName);

	}

	public void scanHDContinuous(String folderPath) {
		Vector<File> files = FileHelper.getFilesFromFolder(folderPath, ".obj");
		for (int i = 0; i < files.size(); i++) {
			File file = files.get(i);
			mesh = new MMesh();
			mesh.loadOBJ(file.getAbsolutePath());

			// mesh.scale(1000, 1000, 1000);
			println("file:" + i + "/" + files.size() + " name"
					+ file.getAbsolutePath());
			mesh.triangulateQuads();
			Box box = mesh.getBounds();
			box = box.getOffsetBox(gridHD.dim * 5);
			box.intersect(boundsScan);
			if (box.y1 < box.y2) {
				Grid3DFloat grid = this.loadPlanes(box.y1, box.y2);
				MMesh.scanMeshDistanceContinue(mesh.xc, mesh.yc, mesh.zc,
						mesh.triangles, grid.x1, grid.y1, grid.z1, grid.nX,
						grid.nY, grid.nZ, grid.dim, grid.dim * 1.6f,
						grid.values);
				this.savePlanes(grid);
			}
		}
		// grid.saveValues(folderPath+"/values"+timeStamp()+"_dim"+grid.dim+".txt");
	}

	public void scanContinues(String folderPath) {
		constructScanBounds();
		float dimX = boundsScan.getDimX();
		float dimY = boundsScan.getDimY();
		float dimZ = boundsScan.getDimZ();
		int nX = (int) (dimX / dimGrid) + 2;
		int nY = (int) (dimY / dimGrid) + 2;
		int nZ = (int) (dimZ / dimGrid) + 2;
		println("nX: " + nX + "  nY: " + nY + "  nZ: " + nZ);
		grid = new Grid3DFloat(nX, nY, nZ);
		grid.x1 = boundsScan.x1;
		grid.y1 = boundsScan.y1;
		grid.z1 = boundsScan.z1;
		grid.dim = dimGrid;
		Arrays.fill(grid.values, 100000);
		scanContinues(folderPath, grid);
	}

	public void scanContinues(String folderPath, Grid3DFloat grid) {
		Vector<File> files = FileHelper.getFilesFromFolder(folderPath, ".obj");
		for (int i = 0; i < files.size(); i++) {
			File file = files.get(i);
			mesh = new MMesh();
			mesh.loadOBJ(file.getAbsolutePath());
			// mesh.scale(1000, 1000, 1000);
			println("file:" + i + "/" + files.size() + " name"
					+ file.getAbsolutePath());
			mesh.triangulateQuads();

			MMesh.scanMeshDistanceContinue(mesh.xc, mesh.yc, mesh.zc,
					mesh.triangles, boundsScan.x1, boundsScan.y1,
					boundsScan.z1, grid.nX, grid.nY, grid.nZ, grid.dim,
					grid.dim * 1.6f, grid.values);
		}
		grid.saveValues(folderPath + "/values" + timeStamp() + "_dim"
				+ grid.dim + ".txt");
	}

	public void initPreviewGrid() {

		gridPreview = new Grid3DFloat(grid.nX / step, grid.nY / step, grid.nZ
				/ step);
		gridPreview.x1 = grid.x1;
		gridPreview.y1 = grid.y1;
		gridPreview.z1 = grid.z1;
		gridPreview.dim = grid.dim * step;

	}

	public void loadPreviewGrid() {
		for (int x = 0; x < gridPreview.nX; x++) {
			for (int y = 0; y < gridPreview.nY; y++) {
				for (int z = 0; z < gridPreview.nZ; z++) {
					gridPreview.set(x, y, z,
							grid.get(x * step, y * step, z * step));
				}
			}
		}
		/*
		 * for (int x=0;x<gridPreview.nX;x++){ for (int
		 * y=0;y<gridPreview.nY;y++){ for (int z=0;z<gridPreview.nZ;z++){
		 * gridPreview.set(x,y,z,grid.get(x*step, y*step, z*step)); } } }
		 */
		Box boundsInsideInt = this.constructIntegerBounds(grid);
		for (int x = 0; x < grid.nX; x++) {
			for (int y = 0; y < grid.nY; y++) {
				for (int z = 0; z < grid.nZ; z++) {
					if (boundsInsideInt.x1 > x || boundsInsideInt.x2 < x
							|| boundsInsideInt.y1 > y || boundsInsideInt.y2 < y
							|| boundsInsideInt.z1 > z || boundsInsideInt.z2 < z) {
						grid.values[x * grid.nY * grid.nZ + y * grid.nZ + z] = grid.dim * 2;
					}
				}
			}
		}

		mcPreview.marchingCubes(gridPreview.nX, gridPreview.nY, gridPreview.nZ,
				gridPreview.values, iso, gridPreview.dim, gridPreview.x1,
				gridPreview.y1, gridPreview.z1, null);
	}

	public void loadBoundingBox(String folderPath) {
		Box b = new Box();
		Vector<File> files = FileHelper.getFilesFromFolder(folderPath, ".obj");
		for (int i = 0; i < files.size(); i++) {
			File file = files.get(i);
			mesh = new MMesh();
			mesh.loadOBJ(file.getAbsolutePath());
			println("file:" + i + "/" + files.size() + " name"
					+ file.getAbsolutePath());
			mesh.triangulateQuads();
			if (i == 0) {
				b = mesh.getBounds();
			} else {
				b.add(mesh.getBounds());
			}
		}
		println(b);
	}

	public void defineInsideOutsideHD(Grid3DFloat hdGrid) {
		// defineInsideOutside();
		grid = new Grid3DFloat(pathValue);
		Box boundsInsideInt = constructIntegerBounds(hdGrid);
		boundsInsideInt.y1 = 0;
		boundsInsideInt.y2 = hdGrid.nY;
		
		// println(" grid dim: ");
		for (int x = 0; x < hdGrid.nX; x++) {
			for (int y = 0; y < hdGrid.nY; y++) {
				for (int z = 0; z < hdGrid.nZ; z++) {
					float xP = hdGrid.getXFloat(x);
					float yP = hdGrid.getYFloat(y);
					float zP = hdGrid.getZFloat(z);

					int xV = grid.getXInt(xP);
					int yV = grid.getYInt(yP);
					int zV = grid.getZInt(zP);
					if (boundsInsideInt.x1 > x || boundsInsideInt.x2 < x
							|| boundsInsideInt.y1 > y || boundsInsideInt.y2 < y
							|| boundsInsideInt.z1 > z || boundsInsideInt.z2 < z) {
						hdGrid.set(x, y, z, 0);

					} else {

						if (grid.get(xV, yV, zV) > iso) {
							// hdGrid.values[x * hdGrid.nY * hdGrid.nZ + y *
							// hdGrid.nZ + z] *= -1;
						}
					}
				}
			}
		}
		float hdIso = hdGrid.dim * 0.85f;
		println("hdIso: " + hdIso);
		float zPos = sz1.getFloatValue() + 0.02f;
		float yPos = (sy1.getFloatValue() + sy2.getFloatValue()) / 2f;
		float xPos = sx2.getFloatValue() - 0.1f;

		int iX = hdGrid.getXInt(xPos);
		int iY = hdGrid.nY / 2;
		int iZ = hdGrid.getZInt(zPos);
		println("inside: " + boundsInsideInt.contains(iX, iY, iZ));
		println("start reach HD");
		int index = hdGrid.getIndex(iX, iY, iZ);
		hdGrid.reached(index, hdIso);
		/*
		 * Grid3DReached.reachedStep(hdGrid.values, hdGrid.nX, hdGrid.nY,
		 * hdGrid.nZ, hdGrid.dim*0.85f);
		 */
		println("end reach");
		// negative reached
		for (int i = 0; i < hdGrid.values.length; i++) {
			if (hdGrid.values[i] < 0)
				hdGrid.values[i] *= -1;// reached
			else if (hdGrid.values[i] > hdIso) {
				hdGrid.values[i] = 0;// inside
			}

		}

		// smaller iso->inside
		for (int x = 0; x < hdGrid.nX; x++) {
			for (int y = 0; y < hdGrid.nY; y++) {
				for (int z = 0; z < hdGrid.nZ; z++) {
					if (hdGrid.get(x, y, z) < iso) {
						if (boundsInsideInt.x1 == x || boundsInsideInt.x2 == x
								|| boundsInsideInt.y1 == y
								|| boundsInsideInt.y2 == y
								|| boundsInsideInt.z1 == z
								|| boundsInsideInt.z2 == z) {
							hdGrid.values[x * hdGrid.nY * hdGrid.nZ + y
									* hdGrid.nZ + z] = 0;
						}
					}
				}
			}
		}
		// crop();

	}

	public void defineInsideOutside() {

		float zPos = sz1.getFloatValue() + 0.02f;
		float yPos = (sy1.getFloatValue() + sy2.getFloatValue()) / 2f;
		float xPos = sx2.getFloatValue() - 0.1f;
		grid = new Grid3DFloat(pathValue);
		Box boundsInsideInt = constructIntegerBounds(grid);
		int iX = grid.getXInt(xPos);
		int iY = grid.getYInt(yPos);
		int iZ = grid.getZInt(zPos);
		println(" pos int: " + iX + " " + iY + " " + iZ);
		println("grid." + grid.nX + " " + grid.nY + " " + grid.nZ);
		int index = grid.getIndex(iX, iY, iZ);

		println("dim: " + grid.dim);

		println("grid pt1: " + grid.x1() + " " + grid.y1() + " " + grid.z1());
		println("grid pt2: " + grid.x2() + " " + grid.y2() + " " + grid.z2());
		initPreviewGrid();
		println(" grid dim: " + grid.x1 + " " + grid.y1 + " " + grid.z1);
		// println(" grid dim: ");
		for (int x = 0; x < grid.nX; x++) {
			for (int y = 0; y < grid.nY; y++) {
				for (int z = 0; z < grid.nZ; z++) {
					if (boundsInsideInt.x1 > x || boundsInsideInt.x2 < x
							|| boundsInsideInt.y1 > y || boundsInsideInt.y2 < y
							|| boundsInsideInt.z1 > z || boundsInsideInt.z2 < z) {
						grid.values[x * grid.nY * grid.nZ + y * grid.nZ + z] = 0;
					} else {
						if (grid.values[x * grid.nY * grid.nZ + y * grid.nZ + z] < 0) {
							grid.values[x * grid.nY * grid.nZ + y * grid.nZ + z] *= -1;
						}
					}
				}
			}
		}

		println("start reach");
		println("grid info");

		grid.reached(index, iso);
		/*
		 * Grid3DFloat.reached(grid.values, grid.nX, grid.nY, grid.nZ, index,
		 * iso);
		 */
		println("end reach");
		// negative reached
		for (int i = 0; i < grid.values.length; i++) {
			if (grid.values[i] < 0)
				grid.values[i] *= -1;// reached
			else if (grid.values[i] > iso)
				grid.values[i] = 0;// unreached
		}

		for (int x = 0; x < grid.nX; x++) {
			for (int y = 0; y < grid.nY; y++) {
				for (int z = 0; z < grid.nZ; z++) {
					if (boundsInsideInt.x1 > x || boundsInsideInt.x2 < x
							|| boundsInsideInt.y1 > y || boundsInsideInt.y2 < y
							|| boundsInsideInt.z1 > z || boundsInsideInt.z2 < z) {
						grid.values[x * grid.nY * grid.nZ + y * grid.nZ + z] = grid.dim * 2;
					}
				}
			}
		}
		// smaller iso->inside
		for (int x = 0; x < grid.nX; x++) {
			for (int y = 0; y < grid.nY; y++) {
				for (int z = 0; z < grid.nZ; z++) {
					if (grid.get(x, y, z) < iso) {
						if (boundsInsideInt.x1 == x || boundsInsideInt.x2 == x
								|| boundsInsideInt.y1 == y
								|| boundsInsideInt.y2 == y
								|| boundsInsideInt.z1 == z
								|| boundsInsideInt.z2 == z) {
							grid.values[x * grid.nY * grid.nZ + y * grid.nZ + z] = 0;
						}
					}
				}
			}
		}
		crop(grid);
		loadPreviewGrid();
	}

	public void cropOld(Grid3DFloat grid) {
		// if (outsideCrop) distance to line
		// if (insideCrop and unreached)

		// smaller iso->inside
		float zM = 1.5f;
		border = new Polygon2D();
		border.addPt(sx2.getFloatValue(), sz2.getFloatValue());
		border.addPt(sx2.getFloatValue(), sz1.getFloatValue());
		border.addPt(sx1.getFloatValue(), sz1.getFloatValue());
		// border.addPt(sx1.getFloatValue(),zM);
		Bezier bez = new Bezier();
		bez.addControlPoint(sx1.getFloatValue(), zM, 0);
		bez.addControlPoint(sx1.getFloatValue(), zM + 1, 0);
		bez.addControlPoint(sx2.getFloatValue() - 1, sz2.getFloatValue(), 0);
		bez.addControlPoint(sx2.getFloatValue(), sz2.getFloatValue(), 0);
		int nSegs = 100;
		for (int i = 0; i < nSegs; i++) {
			PVector p = bez.getPoint(i * 1f / nSegs);
			border.addPt(p.x, p.y);
		}

		border = new Polygon2D();
		border.addPt(sx2.getFloatValue(), sz2.getFloatValue());
		border.addPt(sx2.getFloatValue(), sz1.getFloatValue());
		// border.addPt(sx1.getFloatValue(),sz1.getFloatValue());
		// border.addPt(sx1.getFloatValue(),zM);
		bez = new Bezier();
		bez.addControlPoint(sx1.getFloatValue(), sz1.getFloatValue(), 0);
		bez.addControlPoint(sx1.getFloatValue(), sz1.getFloatValue()+1, 0);
		bez.addControlPoint(sx1.getFloatValue(), sz1.getFloatValue()+1, 0);
		bez.addControlPoint(sx1.getFloatValue(), sz1.getFloatValue()+1.5f, 0);
		bez.addControlPoint(sx2.getFloatValue()-1, sz2.getFloatValue(), 0);
		//bez.addControlPoint(sx1.getFloatValue()-1,sz2.getFloatValue(),0);
		bez.addControlPoint(sx2.getFloatValue(), sz2.getFloatValue(), 0);
		nSegs = 100;
		for (int i = 0; i < nSegs; i++) {
			PVector p = bez.getPoint(i * 1f / nSegs);
			border.addPt(p.x, p.y);
		}
		GeneralPath area = border.getGeneralPath();

		float[][] inoutLayer = new float[grid.nX][grid.nZ];
		for (int x = 0; x < grid.nX; x++) {
			for (int z = 0; z < grid.nZ; z++) {
				float xP = grid.getXFloat(x);
				float yP = grid.getZFloat(z);
				float d = border.getClosestDist(xP, yP);
				if (!area.contains(xP, yP)) {
					inoutLayer[x][z] = border.getClosestDist(xP, yP);
				}
			}
		}

		// cropping everything that is outside
		for (int y = 0; y < grid.nY; y++) {
			for (int x = 0; x < grid.nX; x++) {
				for (int z = 0; z < grid.nZ; z++) {
					if (grid.get(x, y, z) < iso) {
						if (inoutLayer[x][z] > 0) {
							grid.set(x, y, z, inoutLayer[x][z]);
						}
					}
				}
			}
		}
	}
	public void cropTopBottom(Grid3DFloat hdGrid){
		Box boundsInsideInt = constructIntegerBounds(hdGrid);
		boundsInsideInt.y1 = 1;
		boundsInsideInt.y2 = hdGrid.nY - 2;
		for (int x = 0; x < hdGrid.nX; x++) {
			for (int y = 0; y < hdGrid.nY; y++) {
				for (int z = 0; z < hdGrid.nZ; z++) {
					if (boundsInsideInt.x1 > x || boundsInsideInt.x2 < x
							|| boundsInsideInt.y1 > y || boundsInsideInt.y2 < y
							|| boundsInsideInt.z1 > z || boundsInsideInt.z2 < z) {
						hdGrid.values[x * hdGrid.nY * hdGrid.nZ + y * hdGrid.nZ
								+ z] = hdGrid.dim * 2;
					}
				}
			}
		}
	}
	public void crop(Grid3DFloat grid) {
		// if (outsideCrop) distance to line
		// if (insideCrop and unreached)

		// smaller iso->inside
		float zM = 1.5f;
		border = new Polygon2D();
		border.addPt(boundsCrop.x2, boundsCrop.z2);
		border.addPt(boundsCrop.x2, boundsCrop.z1);
		border.addPt(boundsCrop.x1, boundsCrop.z1);
		// border.addPt(boundsCrop.x1,zM);
		Bezier bez = new Bezier();
		bez.addControlPoint(boundsCrop.x1, zM, 0);
		bez.addControlPoint(boundsCrop.x1, zM + 1, 0);
		bez.addControlPoint(boundsCrop.x2 - 1, boundsCrop.z2, 0);
		bez.addControlPoint(boundsCrop.x2, boundsCrop.z2, 0);
		int nSegs = 100;
		for (int i = 0; i < nSegs; i++) {
			PVector p = bez.getPoint(i * 1f / nSegs);
			border.addPt(p.x, p.y);
		}

		border = new Polygon2D();
		border.addPt(boundsCrop.x2, boundsCrop.z2-0.2f);
		border.addPt(boundsCrop.x2, boundsCrop.z1);
		// border.addPt(boundsCrop.x1,boundsCrop.z1);
		// border.addPt(boundsCrop.x1,zM);
		bez = new Bezier();
		bez.addControlPoint(boundsCrop.x1, boundsCrop.z1, 0);
		bez.addControlPoint(boundsCrop.x1, boundsCrop.z1+1, 0);
		bez.addControlPoint(boundsCrop.x1, boundsCrop.z1+1, 0);
		bez.addControlPoint(boundsCrop.x1, boundsCrop.z1+1.5f, 0);
		bez.addControlPoint(boundsCrop.x2-1, boundsCrop.z2-0.2f, 0);
		//bez.addControlPoint(boundsCrop.x1-1,boundsCrop.z2,0);
		bez.addControlPoint(boundsCrop.x2, boundsCrop.z2-0.2f, 0);
		nSegs = 100;
		for (int i = 0; i < nSegs; i++) {
			PVector p = bez.getPoint(i * 1f / nSegs);
			border.addPt(p.x, p.y);
		}
		GeneralPath area = border.getGeneralPath();

		float[][] inoutLayer = new float[grid.nX][grid.nZ];
		for (int x = 0; x < grid.nX; x++) {
			for (int z = 0; z < grid.nZ; z++) {
				float xP = grid.getXFloat(x);
				float yP = grid.getZFloat(z);
				float d = border.getClosestDist(xP, yP);
				if (!area.contains(xP, yP)) {
					inoutLayer[x][z] = border.getClosestDist(xP, yP);
				}
			}
		}

		// cropping everything that is outside
		for (int y = 0; y < grid.nY; y++) {
			for (int x = 0; x < grid.nX; x++) {
				for (int z = 0; z < grid.nZ; z++) {
					if (grid.get(x, y, z) < iso) {
						if (inoutLayer[x][z] > 0) {
							//grid.set(x, y, z, inoutLayer[x][z]);
						}
					}
				}
			}
		}
		
		// cropping everything that is outside
		for (int y = 0; y < grid.nY; y++) {
			float zP=grid.getYFloat(y);
			Polygon2D cBorder=getBorder(zP);
			GeneralPath cArea = cBorder.getGeneralPath();
			println("y: "+y);
			for (int x = 0; x < grid.nX; x++) {
				for (int z = 0; z < grid.nZ; z++) {
					
					if (grid.get(x, y, z) < iso) {
						float xP = grid.getXFloat(x);
						float yP = grid.getZFloat(z);
						
						if (!cArea.contains(xP, yP)){
							float d = cBorder.getClosestDist(xP, yP);
							if (d>0){
								grid.set(x, y, z, d);
							}
						}
						/*if (inoutLayer[x][z] > 0) {
							
						}*/
					}
				}
			}
		}
	}
	public Polygon2D getBorder(float y){
		
			float bumpY=0.22f;
			float z2 = boundsCrop.z2 -bumpY;
			float wave = 1.7f;
			float amp = bumpY/2f;
			float startY = -0.5f;
			float dZ = 0;
			float cY = y - startY;
			if (cY > 0 && cY < wave) {
				dZ = sin(cY * 2 * PI / wave - PI / 2f) * amp + amp;
			}
			Polygon2D border = new Polygon2D();
			border.addPt(boundsCrop.x2, z2);
			border.addPt(boundsCrop.x2, boundsCrop.z1);
			border.addPt(boundsCrop.x1,boundsCrop.z1);
			int nSegs = 200;
			float r = boundsCrop.getDimX();
			float sY=1;
			float scaleY=(boundsCrop.getDimZ()-sY-bumpY)/r;
			float bumpXStart=-1f;
			float bumpXEnd=0;
			float bumpDimX=bumpXEnd-bumpXStart;
			for (int iX = 0; iX < nSegs; iX++) {
				float cX = iX * r / nSegs;
				float dY = sqrt(sq(r) - sq(r-cX) )*scaleY+sY;
				float pX = cX + boundsCrop.x1;
				float pY = dY+boundsCrop.z1;
				
				if (pX>bumpXStart){
					float deltaX=(pX-bumpXStart)/bumpDimX;
					float newdeltaX=1-abs(pow(bumpDimX-deltaX,2f))/abs(pow(bumpDimX,2f));
					deltaX=(deltaX+1*newdeltaX)/2f;
					float addY=(sin(PI*deltaX-HALF_PI)+1f)/2f;
					pY+=dZ*addY;
				}
				border.addPt(pX, pY);
			}
			return border;
		
	}
	
	public void saveSegmentsHD() {
		int nSegs = 8;
		float dY = (sy2.getFloatValue() - sy1.getFloatValue()) / nSegs;
		for (int i = 0; i < nSegs; i++) {
			float y1 = sy1.getFloatValue() + i * dY;
			float y2 = sy1.getFloatValue() + (i + 1) * dY;
			Grid3DFloat seg = this.loadPlanes(y1, y2);
			addMirrorX(seg);
			this.defineInsideOutsideHD(seg);
			crop(seg);
			addScaled(seg);
			splitSegments(seg);
			offset(seg);
			cropTopBottom(seg);
			mc.marchingCubes(seg.nX, seg.nY, seg.nZ, seg.values,
					seg.dim * 0.85f, seg.dim, seg.x1, seg.y1, seg.z1, path
							+ "segtest-" + i + "_" + timeStamp() + ".obj");
		}

	}
	public void splitSegments(Grid3DFloat grid){
		//from z1 to z2
		float dimX=boundsCrop.x2-boundsCrop.x1;
		int nX=(int)(dimX/0.3f);
		float realDX=dimX/nX;
		for (int x=1;x<nX;x++){
			int cx=grid.getXInt(boundsCrop.x1+x*realDX);
			for (int y=0;y<grid.nY;y++){
				for (int z=0;z<grid.nZ;z++){
					grid.set(cx, y, z, grid.dim);
					grid.set(cx+1, y, z, grid.dim);
				}
			}
		}
		
		float dimZ=boundsCrop.z2-boundsCrop.z1;
		int nZ=(int)(dimZ/0.3f);
		float realDZ=dimZ/nZ;
		for (int z=1;z<nZ;z++){
			int cz=grid.getZInt(boundsCrop.z1+z*realDZ);
			for (int x=0;x<grid.nX;x++){
				for (int y=0;y<grid.nY;y++){
					grid.set(x, y, cz, grid.dim);
					grid.set(x, y, cz+1, grid.dim);
				}
			}
		}
	}

	public void saveSegments() {
		Box box = constructIntegerBounds(grid);
		int nSegs = 7;
		int nY = (int) box.getDimY() / nSegs;
		Grid3DFloat seg = new Grid3DFloat(grid.nX, nY, grid.nZ);
		seg.x1 = grid.x1;
		seg.z1 = grid.z1;
		seg.dim = grid.dim;
		for (int i = 0; i < nSegs; i++) {
			int y1 = i * nY + (int) box.y1;
			int y2 = (i + 1) * nY + (int) box.y1;
			seg.y1 = grid.getYFloat(y1);
			for (int x = 0; x < grid.nX; x++) {
				for (int z = 0; z < grid.nZ; z++) {

					for (int cY = y1; cY < y2; cY++) {

						seg.set(x, cY - y1, z, grid.get(x, cY, z));
						if (cY == y1 || cY == y2 - 1) {
							seg.set(x, cY - y1, z, iso * 2);
						}
					}
				}
			}

			mc.marchingCubes(seg.nX, seg.nY, seg.nZ, seg.values,
					seg.dim * 0.85f, seg.dim, seg.x1, seg.y1, seg.z1, path
							+ "seg" + i + "-" + timeStamp() + ".obj");
		}

	}

	public void draw() {
		this.updateBoundsCrop();
	
		if (flagUpdateInsideOutside) {
			flagUpdateInsideOutside = false;

			defineInsideOutside();
			mc.marchingCubes(grid.nX, grid.nY, grid.nZ, grid.values,
					grid.dim * 0.85f, grid.dim, grid.x1, grid.y1, grid.z1, null);
		}
		if (flagExport) {
			flagExport = false;
			mc.saveObj(path+timeStamp()+".obj");
			//saveSegments();
		}
		drawData();
		cam.beginHUD();
		noLights();
		drawGui();
		cam.endHUD();
	}

	public void drawData() {
		background(0);
		pushMatrix();
		// cam.apply((PGraphics) g);
		rotateY(PI + rY);
		hint(ENABLE_DEPTH_TEST);

		float dimX = grid.x2() - grid.x1();
		scale(3 / dimX * 50);
		scale(1, -1, 1);
		PCamera3D.drawWorldAxis3D(g, 5);
		pointLight(170, 170, 170, 3, 3, 3);
		pointLight(170, 170, 170, -3, -3, -3);
		if (displayMode == 0) {
			noFill();
			stroke(0, 255, 0);
			// mesh.draw((PGraphics3D) g);
			/*
			 * beginShape(TRIANGLES); for (int i = 0; i < mesh.triangles.length;
			 * i += 3) { for (int j = 0; j < 3; j++) {
			 * vertex(mesh.xc[mesh.triangles[i + j]], mesh.yc[mesh.triangles[i +
			 * j]], mesh.zc[mesh.triangles[i + j]]); } } endShape();
			 */
		}
		noStroke();
		fill(255);
		// mc.draw((PGraphics) g);
		mcPreview.draw(g);
		noFill();
		stroke(0, 0, 255);
		// the bounds
		Box boundsInsideInt = this.constructIntegerBounds(grid);
		translate(grid.x1, grid.y1, grid.z1);
		scale(grid.dim);
		translate(boundsInsideInt.getCenterX(), boundsInsideInt.getCenterY(),
				boundsInsideInt.getCenterZ());
		box(boundsInsideInt.getDimX(), boundsInsideInt.getDimY(),
				boundsInsideInt.getDimZ());
		popMatrix();
	}

	public void drawGui() {
		noLights();
		hint(DISABLE_DEPTH_TEST);
		fill(10);
		stroke(100);
		rect(0, 0, 100, height);
		gui.display(g);
	}

	// returns a string encoding the current time
	public String timeStamp() {
		return year() + nf(month(), 2) + nf(day(), 2) + "-" + nf(hour(), 2)
				+ nf(minute(), 2) + nf(second(), 2);
	}

	public void keyPressed() {
		if (key == 'u') {
			flagUpdateInsideOutside = true;
		}
		if (key == 'd') {
			displayMode++;
			displayMode = displayMode % 3;
		}
		if (key == 'e') {
			flagExport = true;
		}
		if (key == 'r') {
			rY -= PI / 4;
		}
	}
}
